/*  电机编号
            头

     9  5       4 8
        1       0
          |dog|
          |dog|
          |dog|
        3       2
     11 7       6 10
*/

#ifndef _STATEESTIMATOR_H_
#define _STATEESTIMATOR_H_

#include "params.h"
#include "global.h"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

/*
    状态估计器

*/

class StateEstimator
{
private:
    double t_; // 当前时间
    int n_; // 当前为第n个周期

    bool IMUsetflag_;
    bool Encoderflag_;
    bool Calculatedflag_;
    // IMU
    Vector3d a_com_B_;     // B系质心加速度
    Vector3d a_com_O_;     // O系质心加速度
    Vector3d omega_OB_B_;  // B系躯干角速度
    Vector3d omega_OB_O_;  // O系躯干角速度
    Matrix3d omega_OBX_B_; // B系躯干角速度张量(2.51)

    Vector3d p_i_B_;     // 本体系下i足足底位置(2.47)
    Vector3d p_dot_i_B_; // 本体系下i足足底速度(2.49)

    Matrix3d R_imu_imu0_;             // imu->imu0旋转矩阵(IMU原始旋转矩阵)
    Matrix<double, 4, 3> q_j_i_;     // 12个关节电机角度,电机编号见顶部
    Matrix<double, 4, 3> q_dot_j_i_; // 12个关节电机角速度,电机编号见顶部
    Vector3d a_original_;             // 原始IMU加速度
    Vector3d omega_original_;         // 原始IMU角速度

    Matrix3d R_B_O_;   // B->O躯干旋转矩阵(通过IMU和世界系的定义计算得出)
    Matrix3d R_B_imu_; // B->imu旋转矩阵(常值)

    Matrix3d R_imu0_O_;      // imu0->O旋转矩阵(常值)
    Matrix3d R_t0_imu_imu0_; // 初始时刻IMU旋转矩阵(IMU启动时读出)

    Matrix<double, 4, 3> p_stend_O_last_; // 上一控制周期求得有效支撑相足底坐标

    // Kalman filter
    Matrix<double, 18, 18> A_;
    Matrix<double, 18, 3> B_;
    Matrix<double, 18, 18> Q_;
    Matrix<double, 28, 28> R_;
    Matrix<double, 28, 18> H_;
    Matrix<double, 18, 18> P_;
    Matrix<double, 18, 28> K_;
    Vector3d u_;
    Matrix<double, 28, 1> z_k_;
    Matrix<double, 18, 1> x_k_; // k时刻状态变量[p_com_O, v_com_O, p_foot_O](18*1)

    Vector3d geta_com_B();    // 计算并返回质心加速度，参数为原始IMU加速度
    Vector3d geta_com_O();    // 经过R_B_O_变换后的质心加速度(2.42)
    Vector3d getomega_OB_B(); // 计算并返回躯干角速度，参数为原始IMU角速度
    Vector3d getomega_OB_O(); // 经过R_B_O_变换后的躯干角速度(2.44)

    Matrix3d getomega_OBX_B();

    // 计算本体系下i足足底位置(2.47)，参数q_j_i为第i条腿上的3个关节电机角度，0-髋电机，1-大腿电机，2-小腿电机
    Vector3d getp_i_B(int i, const Vector3d &q_j_i);
    // 计算本体系下i足足底速度(2.49)，参数q_dot_j_i为第i条腿上的3个关节电机速度，0-髋电机，1-大腿电机，2-小腿电机
    Vector3d getp_dot_i_B(int i, const Vector3d &q_j_i, const Vector3d &q_dot_j_i);

public:
    StateEstimator();
    ~StateEstimator();


    // void setIMUdata(const Matrix3d &R_imu_imu0, const Vector3d &a_original, const Vector3d &omega_original);
    
    // 设置InertialUnit输出的欧拉角(原始旋转矩阵)，Gyro输出的角速度，Accelerometer输出的加速度
    void setIMUdata(const Vector3d &Eulerangles, const Vector3d &a_original, const Vector3d &omega_original);
    // 设置12个关节电机角度和角速度
    void setEncoderdata(const Matrix<double, 4, 3> &q_j_i, const Matrix<double, 4, 3> &q_dot_j_i);
    // 计算所有过程变量
    void calculateAll();

    // 卡尔曼滤波计算x_k
    void kalman_update();

    // 计算并返回躯干旋转矩阵，对于spot来说即为原始IMU旋转矩阵
    Matrix3d getR_B_O();

    Matrix<double, 4, 3> getp_stend_O(const Vector4d &s_fai);

    // 返回18*1向量，为[p_com_O(3*1), v_com_O(3*1), p_foot_O(12*1)]
    Matrix<double, 18, 1> getx_k();

    Matrix<double, 4, 3> getp_foot();
};

#endif